from point import Point
from rob import Rob
from mechanism import Mechanism
import sympy as sp
from sympy.abc import t

class RRP(Mechanism):
  """
  Attrs:
    p0：内副C点
    p1:外接副1，B点
    p2：外接副2：D点
    pk:参考点K，默认为导路基点
    rob1：BC杆
    rob2：CD杆
    road：KD导轨
    phi1：以B基点，CB倾角
    phi2：C为基点，CD倾角
    phiroad：导路KD倾角
    s:D点至基点距离
  """
  def __init__(self,p1:Point,road:Rob,l1,l2,pk = None):
    """
    Args: 
        p1:外接副1
        road:导路
        l1,l2:杆长
        pk:可选，导路上的参考点
    """
    print('初始化RRP杆组')

    self.p1 = p1;self.road = road
    self.l1 = l1;self.l2 = l2
    self.symbol_phiroad = road.symbol_phi  # 导路倾角

    self.pk = pk if(pk) else road.basePoint
    
    a = (p1.symbol_rx - self.pk.symbol_rx)*sp.sin(self.symbol_phiroad) - (p1.symbol_ry - self.pk.symbol_ry)*sp.cos(self.symbol_phiroad)
    # 装配条件
    self.minus = sp.lambdify(t,sp.Abs(a+l2)-l1,'numpy')
    assert self.minus(0)<=0 ,"RRP无法装配：|a-l2|-l1必须<=0"

    self.symbol_phi1 = sp.asin((a+l2)/l1)+self.symbol_phiroad
    self.rob1 = Rob(l1,p1,self.symbol_phi1)
    self.p0 = self.rob1.endPoint# 内副
    # 确定杆cd
    # self.symbol_phi2 = sp.pi/2 + road.symbol_phi # CD杆倾角,D基点
    # self.s = (self.p0.symbol_ry - self.pk.symbol_ry - l2*sp.cos(self.symbol_phiroad))/sp.sin(self.symbol_phiroad)
    # self.p2 = Point(rx= self.pk.symbol_rx+self.s*sp.cos(self.symbol_phiroad),
    #                 ry= self.pk.symbol_ry+self.s*sp.sin(self.symbol_phiroad))
    # self.rob2 = Rob(l2,self.p2,self.symbol_phi2,endPoint=self.p0)
    
    self.symbol_phi2 = - sp.pi/2 + road.symbol_phi # CD杆倾角,C基点
    self.rob2 = Rob(l2,self.p0,self.symbol_phi2)
    
    self.update(0)
    print('RRP杆组初始化完成')

  def update(self,t_s):
    """
    依据时间更新构件状态
    """
    assert self.minus(0)<=0 ,"RRP无法装配：|a-l2|-l1必须<=0"
    self.road.update(t_s)
    self.rob1.update(t_s)
    self.rob2.update(t_s)
  
  def get_robs(self):
    """
    self.rob1,self.rob2,self.road
    """
    return self.rob1,self.rob2,self.road


if __name__=="__main__":
  p1 = Point(0,0)
  p2 = Point(50,0)
  r = Rob(300,p1,t)
  m2 = RRP(p2,r,300,300)